package com.yhy.wxzn.wcs.device.rgv.core.avoid;


import com.yhy.wxzn.wcs.SystemInit;
import com.yhy.wxzn.wcs.device.rgv.bean.FourWayRgv;
import com.yhy.wxzn.wcs.device.rgv.bean.Node;
import com.yhy.wxzn.wcs.device.rgv.core.lock.LockNodeManager;
import com.yhy.wxzn.wcs.device.rgv.enums.NodeStatus;
import com.yhy.wxzn.wcs.device.rgv.enums.NodeType;
import lombok.Data;
import org.springframework.beans.BeanUtils;


import java.util.*;

public class AvoidHandler {

    private Queue<Node> open = new LinkedList<>();
    private Queue<RecordInfo> record = new LinkedList<>();
    private Set<Node> close = new HashSet<>();
    private HashMap<Node,Node> map;

    private boolean flag;
    private Node res;



    public AvoidHandler( List<Node> map){
        this.map = new HashMap(Math.max((int) (map.size()/.75f) + 1, 16));
        map.forEach(m->this.map.put(m,m));
    }


    /**
     * 该方法只能调用一次
     *  获取最近的避让点：第一次不通过其他车辆，未获取到避让点第二次通过其他车辆
     * @param targetRgv
     * @return
     */
    public Node getNearestAvoidNode(FourWayRgv targetRgv){
        //Node r;
        //todo 托盘状态应该是动态的
        //return (r = bfs1(targetRgv.getCurLocation(),true,false)) == null ? bfs1(targetRgv.getCurLocation(),true,true) : r;
        return bfs1(targetRgv.getCurLocation(),true,false);

    }
    public Node getNearestAvoidNode2(FourWayRgv targetRgv){
        Node r;
        //todo 托盘状态应该是动态的
        return (r = bfs1(targetRgv.getCurLocation(),true,false)) == null ? bfs1(targetRgv.getCurLocation(),true,true) : r;
    }

    //Node需要从数据库读取
    public Node bfs1(Node target, boolean palletStatus, boolean crossRgv) {
        initData();

        record.offer(new RecordInfo(target));
        open.offer(target);

        while (!flag && !open.isEmpty()) addNeighborNodeInOpen(palletStatus,crossRgv);

        return res;
    }

    private void initData() {
        this.open.clear();
        this.close.clear();
        this.record.clear();
        this.flag = false;
        this.res = null;
    }


    private void addNeighborNodeInOpen(boolean palletStatus, boolean crossRgv) {
        Node curNode = open.poll();
        RecordInfo curRecordInfo = record.poll();
        close.add(curNode);

        if (curRecordInfo.getCrossRouteNum() > 0
                && Objects.equals(curNode.getNodeType(),NodeType.P)
                && bfs2(curNode)){
                this.flag = true;
                this.res = curNode;
                return;
        }

        if (curNode.getUp()){
            addNeighborNodeInOpen(curRecordInfo, Node.getUpNode(curNode), palletStatus,crossRgv);
        }
        if (curNode.getDown()){
            addNeighborNodeInOpen(curRecordInfo, Node.getDownNode(curNode), palletStatus,crossRgv);
        }
        if (curNode.getLeft()){
            addNeighborNodeInOpen(curRecordInfo, Node.getLeftNode(curNode), palletStatus,crossRgv);
        }
        if (curNode.getRight()){
            addNeighborNodeInOpen(curRecordInfo, Node.getRightNode(curNode), palletStatus,crossRgv);
        }
    }


    private boolean bfs2(Node node) {
        Queue<Node> open = new LinkedList<>();
        open.offer(node);
        while (!open.isEmpty()){
            Node curNode = open.poll();
            close.add(curNode);
            Node next;
            if (currentNodeExistRgv(curNode)) return false;

            if (curNode.getUp()){
                next = checkNodeInMap(Node.getUpNode(curNode));
                if (canAddNodeToOpenBfs2(next))  open.offer(next);
            }

            if (curNode.getDown()){
                next = checkNodeInMap(Node.getDownNode(curNode));
                if (canAddNodeToOpenBfs2(next))  open.offer(next);
            }
            if (curNode.getLeft()){
                next = checkNodeInMap(Node.getLeftNode(curNode));
                if (canAddNodeToOpenBfs2(next))  open.offer(next);
            }

            if (curNode.getRight()){
                next = checkNodeInMap(Node.getRightNode(curNode));
                if (canAddNodeToOpenBfs2(next))  open.offer(next);
            }
        }
        return true;
    }

    private boolean currentNodeExistRgv(Node curNode) {
        /**
         * test用模拟车辆
         */
//        WcsMapInfoServiceImpl bean = SpringUtil.getBean(WcsMapInfoServiceImpl.class);
//        Node node = bean.getNode(curNode);
//        if (node.getNodeStatus().equals(NodeStatus.Y)) return true;
        for (FourWayRgv rgv : SystemInit.fourWayRgvs) {
            if (Objects.equals(rgv.getCurLocation(),curNode)) return true;
        }
        return false;
    }


    private void addNeighborNodeInOpen(RecordInfo curRecordInfo, Node next, boolean palletStatus, boolean crossRgv) {
        if ((next = checkNodeInMap(next)) == null)
            return;

        if (canAddNodeToOpen(next,palletStatus,crossRgv)) {

            open.offer(next);

            record.offer(new RecordInfo(next,curRecordInfo));

        }
    }


    private Node checkNodeInMap(Node next) {
        if (map.containsKey(next)){
            BeanUtils.copyProperties(map.get(next), next);
            return next;
        }

        return null;
    }

    /**
     * 判断结点能否放入Open列表
     */
    private boolean canAddNodeToOpenBfs2( Node node) {
        if (node == null) return false;

        if (!Objects.equals(node.getNodeType(),NodeType.P))
            return false;


        // 判断结点是否存在close表
        if (isCoordInClose(node))
            return false;


        return true;
    }


    /**
     * 判断结点能否放入Open列表
     */
    private boolean canAddNodeToOpen(Node node, boolean palletStatus, boolean crossRgv) {
        //节点是禁用
        if (Objects.equals(node.getNodeType(), NodeType.B))
            return false;


        // 判断当前是否是载货行驶，并该节点状态为有货状态
        if (palletStatus && Objects.equals(node.getNodeStatus(), NodeStatus.Y) && !node.getNodeType().equals(NodeType.CONNECTION))
            return false;

        //避让点，避免再次发生避让
        if (!crossRgv && LockNodeManager.checkNodesLocked(node)) {
            return false;
        }


        // 判断结点是否存在close表
        if (isCoordInClose(node))
            return false;


        return true;
    }

    private boolean isCoordInClose(Node curNode) {
        return close.contains(curNode);
    }


    @Data
    static class RecordInfo{
        int crossRouteNum;
        List<Node> path;

        public RecordInfo(Node curNode) {
            this.path = Arrays.asList(curNode);
            this.crossRouteNum = Objects.equals(curNode.getNodeType(),NodeType.R) ? 1 : 0;
        }

        public RecordInfo(Node curNode, RecordInfo curRecordInfo) {
            ArrayList<Node> path = new ArrayList<>();
            path.addAll(curRecordInfo.getPath());
            path.add(curNode);

            this.path = path;
            this.crossRouteNum = Objects.equals(curNode.getNodeType(),NodeType.R) ? curRecordInfo.getCrossRouteNum() + 1: curRecordInfo.getCrossRouteNum();
        }
    }

}
